#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S3,     Seeker,         sensorHiTechnicIRSeeker1200)
#pragma config(Motor,  mtr_S1_C1_1,     Right,         tmotorTetrix, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     Left,          tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     motorE,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     ForkLift,      tmotorTetrix, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//#include "C:\Users\Public\Ftc\drivers\2.3\drivers\HTIRS2-driver.h"
#include "JoystickDriver.c"

int motorPower = 70;
int locBeacon = 0;

task main ()
{
	//int lastValue = 0;
	waitForStart();

	//drive away from the wall
	nMotorEncoder[Left]=0;
	nMotorEncoder[Right]=0;
	nMotorEncoder[ForkLift]=0;

	wait1Msec(100);

	while (nMotorEncoder[Left] < 2840)
	{
		motor[Left] = motorPower;
		motor[Right] = motorPower;
	}

	motor[Left]=0;  //stop
	motor[Right]=0;
	wait1Msec(1000);

	//move the arm up to the bottom peg
	nMotorEncoder[ForkLift] = 0;
	wait1Msec(100);

	while(nMotorEncoder[ForkLift] < 2146)
	{
		motor[ForkLift] = motorPower;
	}

	motor[ForkLift] =0;
	wait1Msec(1000);

	/*//where is the beacon?
	locBeacon = SensorValue[Seeker];
	writeDebugStreamLine("%d", locBeacon);
	wait1Msec(50);*/
	//backup a bit
	nMotorEncoder[Left] = 0;
	nMotorEncoder[Right] = 0;
	wait1Msec(100);

	while(nMotorEncoder[Left] > -1000)
	{
		motor[Left] = -motorPower;
		motor[Right] = -motorPower;
		if(locBeacon == 0 && SensorValue[Seeker] != 0)
		{
			locBeacon = SensorValue[Seeker];
		}
	}

	motor[Left] = 0;
	motor[Right] = 0;
	wait1Msec(1000);

	//turn left or right
	nMotorEncoder[Left]=0;
	nMotorEncoder[Right]=0;
	wait1Msec(100);

	if(locBeacon > 5) //turn to right post
	{
		while (nMotorEncoder[Left] < 400)
		{
			motor[Left] = motorPower;
			motor[Right] = -motorPower;
		}

		motor[Left] = 0;
		motor[Right] = 0;
		wait1Msec(100);

		nMotorEncoder[Left]=0;
		nMotorEncoder[Right]=0;
		wait1Msec(100);

		while(nMotorEncoder[Left] < 1440 * 23/(4 *PI))
		{
			motor[Left] = 50;
			motor[Right] = 50;
		}

		motor[Left] = 0;
		motor[Right] = 0;
		wait1Msec(1000);
	}
	else if(locBeacon < 5)  //turn to left post
	{
		while (nMotorEncoder[Left] > -400)
		{
			motor[Left] = -motorPower;
			motor[Right] = motorPower;
		}

		motor[Left] = 0;
		motor[Right] = 0;
		wait1Msec(100);

		nMotorEncoder[Left]=0;
		nMotorEncoder[Right]=0;
		wait1Msec(100);

		while(nMotorEncoder[Left] < 1440 * 18/(4 *PI))
		{
			motor[Left] = 50;
			motor[Right] = 50;
		}

		motor[Left] = 0;
		motor[Right] = 0;
		wait1Msec(1000);
	}

	//moving to the left
	if(locBeacon < 5)
	{
		/*while(SensorValue[Seeker] > 5)
		{
			motor[Left] = 0;
			motor[Right] = -motorPower;
		}
		motor[Left] = 0;
		motor[Right] = 0;
		wait1Msec(100);*/

		nMotorEncoder[Left] = 0;
		nMotorEncoder[Right] = 0;
		wait1Msec(100);

		while(nMotorEncoder[Left] < 410)
		{
			motor[Left] = motorPower;
			motor[Right] = -motorPower;
		}

		motor[Left] = 0;
		motor[Right] = 0;
		wait1Msec(100);

		ClearTimer(T1);
		while(time1[T1] < 1000)
		{
			motor[Left] = motorPower;
			motor[Right] = motorPower;
		}

		motor[Left] = 0;
		motor[Right] = 0;
		wait1Msec(100);

		while(nMotorEncoder[ForkLift] > 0)
		{
			motor[ForkLift] = -motorPower;
		}

		motor[ForkLift] = 0;
		wait1Msec(100);

		nMotorEncoder[Left] = 0;

		while(nMotorEncoder[Left] > -1400)
		{
			motor[Left] = -50;
			motor[Right] = -50;
		}

		motor[Left] = 0;
		motor[Right] = 0;
		wait1Msec(100);

		nMotorEncoder[Left] = 0;

		while(nMotorEncoder[Left] > -500)
		{
			motor[Left] = -50;
			motor[Right] = 50;
		}

		motor[Left] = 0;
		motor[Right] = 0;
		wait1Msec(100);

		nMotorEncoder[Left] = 0;

		while(nMotorEncoder[Left] < 2300)
		{
			motor[Left] = 50;
			motor[Right] = 50;
		}

		motor[Left] = 0;
		motor[Right] = 0;
		wait1Msec(100);

		nMotorEncoder[Left] = 0;

		while(nMotorEncoder[Left] < 830)
		{
			motor[Left] = 50;
			motor[Right] = -50;
		}

		motor[Left] = 0;
		motor[Right] = 0;
		wait1Msec(100);

		nMotorEncoder[Left] = 0;

		while(nMotorEncoder[Left] < 6000)
		{
			motor[Left] = 50;
			motor[Right] = 50;
		}

		motor[Left] = 0;
		motor[Right] = 0;
		wait1Msec(100);

	}
	else if(locBeacon > 5)		//Moving to the right
	{
		/*while(SensorValue[Seeker] < 5)
		{
			motor[Left] = -motorPower;
			motor[Right] = 0;
		}
		motor[Left] = 0;
		motor[Right] = 0;
		wait1Msec(100);*/

		nMotorEncoder[Left] = 0;
		nMotorEncoder[Right] = 0;
		wait1Msec(100);

		while(nMotorEncoder[Left] > -400)
		{
			motor[Left] = -motorPower;
			motor[Right] = motorPower;
		}

		motor[Left] = 0;
		motor[Right] = 0;
		wait1Msec(100);

		ClearTimer(T1);
		while(time1[T1] < 1080)
		{
			motor[Left] = motorPower;
			motor[Right] = motorPower;
		}

		motor[Left] = 0;
		motor[Right] = 0;
		wait1Msec(100);

		while(nMotorEncoder[ForkLift] > 0)
		{
			motor[ForkLift] = -motorPower;
		}

		motor[ForkLift] = 0;
		wait1Msec(100);

		nMotorEncoder[Left] = 0;

		while(nMotorEncoder[Left] > -1400)
		{
			motor[Left] = -50;
			motor[Right] = -50;
		}

		motor[Left] = 0;
		motor[Right] = 0;
		wait1Msec(100);

		nMotorEncoder[Left] = 0;

		while(nMotorEncoder[Left] < 500)
		{
			motor[Left] = 50;
			motor[Right] = -50;
		}

		motor[Left] = 0;
		motor[Right] = 0;
		wait1Msec(100);

		nMotorEncoder[Left] = 0;

		while(nMotorEncoder[Left] < 2300)
		{
			motor[Left] = 50;
			motor[Right] = 50;
		}

		motor[Left] = 0;
		motor[Right] = 0;
		wait1Msec(100);

		nMotorEncoder[Left] = 0;

		while(nMotorEncoder[Left] > -830)
		{
			motor[Left] = -50;
			motor[Right] = 50;
		}

		motor[Left] = 0;
		motor[Right] = 0;
		wait1Msec(100);

		nMotorEncoder[Left] = 0;

		while(nMotorEncoder[Left] < 6000)
		{
			motor[Left] = 50;
			motor[Right] = 50;
		}

		motor[Left] = 0;
		motor[Right] = 0;
		wait1Msec(100);

	}
	else
	{
		// go forward to the correct post (this needs to be a bit less)
//		nMotorEncoder[Left]=0;
//		nMotorEncoder[Right]=0;
		int LEncoderOffset = nMotorEncoder[Left];
		int REncoderOffset = nMotorEncoder[Right];
		wait1Msec(100);

		motor[Right] = 60;
		wait1Msec(42);
		while((nMotorEncoder[Left] - LEncoderOffset) < 1440 * 32/(4 * PI))
		{
			motor[Left] = 50;
			motor[Right] = 50;
		}

		motor[Left] = 0;
		motor[Right] = 0;
		wait1Msec(1000);

		while(nMotorEncoder[ForkLift] > 0)
		{
			motor[ForkLift] = -motorPower;
		}

		motor[ForkLift] = 0;
		wait1Msec(100);

		nMotorEncoder[Left] = 0;

		while(nMotorEncoder[Left] > -400)
		{
			motor[Left] = -50;
			motor[Right] = -50;
		}

		motor[Left] = 0;
		motor[Right] = 0;
		wait1Msec(100);

		LEncoderOffset = nMotorEncoder[Left];
		REncoderOffset = nMotorEncoder[Right];
		wait1Msec(100);

		motor[Right] = -60;
		wait1Msec(35);
		while((nMotorEncoder[Left] - LEncoderOffset) > -1440 * 32/(4 * PI))
		{
			motor[Left] = -50;
			motor[Right] = -50;
		}

		motor[Left] = 0;
		motor[Right] = 0;
		wait1Msec(100);

		nMotorEncoder[Left] = 0;

		while(nMotorEncoder[Left] < 400)
		{
			motor[Left] = 50;
			motor[Right] = -50;
		}

		motor[Left] = 0;
		motor[Right] = 0;
		wait1Msec(100);

		nMotorEncoder[Left] = 0;

		while(nMotorEncoder[Left] < 6000)
		{
			motor[Left] = 50;
			motor[Right] = 50;
		}

		motor[Left] = 0;
		motor[Right] = 0;
		wait1Msec(100);

		nMotorEncoder[Left] = 0;

		while(nMotorEncoder[Left] > -830)
		{
			motor[Left] = -50;
			motor[Right] = 50;
		}

		motor[Left] = 0;
		motor[Right] = 0;
		wait1Msec(100);

		nMotorEncoder[Left] = 0;

		while(nMotorEncoder[Left] < 6000)
		{
			motor[Left] = 50;
			motor[Right] = 50;
		}

		motor[Left] = 0;
		motor[Right] = 0;
		wait1Msec(100);

	}
	// IR Seeker
	/*
	while(1 == 1)
	{
	if(SensorValue[Seeker] == 5)	//If the sensor is lined up with the beacon...
	{
	//...drive straight.
	//lastValue = SensorValue[Seeker];
	motor[Left] = motorPower/2;
	motor[Right] = motorPower/2;
	}
	else if(SensorValue[Seeker] == 0)	//If the sensor is lined up with the beacon...
	{
	//...do nothing
	// spurrious "0"
	motor[Left] = 0;
	motor[Right] = 0;
	}
	else if(SensorValue[Seeker] > 5)	//If the right side of the sensor detects the beacon...
	{
	nMotorEncoder[Left]=0;
	nMotorEncoder[Right]=0;
	wait1Msec(100);
	//...turn right.
	//lastValue = SensorValue[Seeker];
	motor[Left] = motorPower;
	motor[Right] = -motorPower;

	while(nMotorEncoder[Left] < 40)
	{
	motor[Left] = motorPower;
	motor[Right] = motorPower;
	}
	}
	else if(SensorValue[Seeker] < 5)	//If the left side of the sensor detects the beacon (or doesn't see it)...
	{
	nMotorEncoder[Left]=0;
	nMotorEncoder[Right]=0;
	wait1Msec(100);
	//...turn left.
	//lastValue = SensorValue[Seeker];
	motor[Left] = -motorPower;
	motor[Right] = motorPower;

	while(nMotorEncoder[Left] < 40)
	{
	motor[Left] = motorPower;
	motor[Right] = motorPower;
	}
	}
	else{
	// do nothing
	}
	}*/
}
